#ifndef __POINTS_PROCESS_H__
#define __POINTS_PROCESS_H__

#include "common/coord_trans.h"

#include <vector>
#include <cmath>
#include <algorithm>

#include <opencv2/opencv.hpp>

using namespace global_path_planner::common;

namespace global_path_planner {
namespace path_smooth{
namespace points {
	/*
	    生成探索矩阵
	*/
	static std::vector<std::vector<float>> GenExploreList(int ksize){
	  	std::vector<std::vector<float>> explore_lists;
	  	std::vector<float> list;
	  	for(int i = 0; i < 2 * ksize + 1; i++){
	  	    for(int j = 0; j < 2 * ksize + 1; j++){
	  	        list.clear();
	  	        int di = i - ksize;
				int dj = j - ksize;
				if(di == 0 && dj == 0) continue;
	  	        list.push_back(di);
	  	        list.push_back(dj);
	  	        list.push_back(std::sqrt(di * di + dj * dj));
	  	        explore_lists.push_back(list);
	  	    }
	  	}
	  	std::sort(explore_lists.begin(), explore_lists.end(),
	  	            [](std::vector<float>& l,
	  	              std::vector<float>& r) -> bool
	  	            {
	  	              return l[2] < r[2];
	  	            });
	  	return explore_lists;
	}

	// 判断点的合法性
	static bool CheckPixelPoint(std::pair<int, int>& pixel_pt,
									cv::Mat& map_grid){
		if (pixel_pt.first < 0 || pixel_pt.first >= map_grid.cols ||
			pixel_pt.second < 0 || pixel_pt.second >= map_grid.rows)
			return false;
		else if (map_grid.at<uchar>(pixel_pt.second, pixel_pt.first) != 255)
			return false;
		return true;
	}

	static bool CheckRealPoint(std::pair<float, float> &point,
								  float resolution, float origin_x,
								  float origin_y, cv::Mat &map_grid)
	{
		auto pixel = coordtrans::Realxy2Pixelxy(point, resolution,
												origin_x, origin_y);
		return CheckPixelPoint(pixel, map_grid);
	}

	// 从当前点找附近合法的点
	static std::pair<bool, std::pair<float, float>>
	GenNearestNode(std::pair<float, float> &point, int& sz,
								float resolution, float origin_x,
								float origin_y, cv::Mat &map_grid)
	{
		// 围绕当前点继续展开探索
		bool __has_Nearest_Node = false;
		std::pair<float, float> tmp_point;
		std::vector<std::vector<float>> __exp_lists = GenExploreList(sz);
		for (auto iter = __exp_lists.begin(); iter != __exp_lists.end(); iter++)
		{
			tmp_point = std::make_pair(point.first + (*iter)[0] * resolution,
									   point.second + (*iter)[1] * resolution);
			if (CheckRealPoint(tmp_point, resolution,
			              	origin_x, origin_y, map_grid))
			{
				__has_Nearest_Node = true;
				break;
			}
		}
		return std::make_pair(__has_Nearest_Node, tmp_point);
	}

	static std::pair<bool, std::pair<int, int>>
	GenNearestNode(std::pair<int, int> &point,
					int& sz, cv::Mat &map_grid)
	{
		// 围绕当前点继续展开探索
		bool __has_Nearest_Node = false;
		std::pair<int, int> tmp_point;
		std::vector<std::vector<float>> __exp_lists = GenExploreList(sz);
		for (auto iter = __exp_lists.begin(); iter != __exp_lists.end(); iter++)
		{
			tmp_point = std::make_pair(point.first + (*iter)[0],
									   point.second + (*iter)[1]);
			if (CheckPixelPoint(tmp_point, map_grid))
			{
				__has_Nearest_Node = true;
				break;
			}
		}
		return std::make_pair(__has_Nearest_Node, tmp_point);
	}

	// 两点之间间隔interval插入点集
	static std::vector<std::tuple<double, double, double>>
	AddDensePoints(std::tuple<double, double, double> &start,
					std::tuple<double, double, double> &end,
					double interval)
	{
		std::vector<std::tuple<double, double, double>> res;
		double dx = std::get<0>(end) - std::get<0>(start);
		double dy = std::get<1>(end) - std::get<1>(start);
		double dphi = std::get<2>(end) - std::get<2>(start);
		double dis = sqrt(dx * dx + dy * dy);
		int point_num = std::ceil(dis / interval);
		bool is_singular = false;
		double k = 0.;
		double b = 0.;
		double l_interval = 0.;
		double phi_interval = dphi / point_num;
		if (std::abs(dx) < 1e-6)
		{
			is_singular = true;
			l_interval = dy / (double)point_num;
		}
		else
		{
			k = dy / dx;
			b = std::get<1>(start) - k * std::get<0>(start);
			l_interval = dx / (double)point_num;
		}
		for (int i = 0; i < point_num; i++)
		{
			double curX = 0.;
			double curY = 0.;
			if (is_singular)
			{
				curX = std::get<0>(start);
				curY = std::get<1>(start) + i * l_interval;
			}
			else
			{
				curX = std::get<0>(start) + i * l_interval;
				curY = k * curX + b;
			}
			if (i == point_num - 1)
			{
				curX = std::get<0>(end);
				curY = std::get<1>(end);
			}
			double curPhi = std::get<2>(start) + i * phi_interval;
			res.emplace_back(std::make_tuple(curX, curY, curPhi));
		}
		return res;
	}
} // points
} // path_smooth
} // global_path_planner

#endif // __POINTS_PROCESS_H__